#include "ms575_alghighse1se9edgeouter.h"
using namespace MeasurE575;

AlgHighSE1SE9EdgeOuter::AlgHighSE1SE9EdgeOuter(int i):AlgBase(i)
{
}

void AlgHighSE1SE9EdgeOuter::operator()(cv::Mat &srcImg,
                                        int id,
                                        std::map<string, float> &measRes,
                                        int fixtureId)
{
    //high SE19-> 18, SE9 -> 21
    //high SE1289 -> 47,48,49,50
    std::vector<int> paras1,paras9, paras2,paras8;
    std::vector<cv::Point2f> pt1Vec,pt9Vec,pt2Vec,pt8Vec;
    std::vector<cv::Rect> rangSE1Vec,rangSE9Vec,rangSE2Vec,rangSE8Vec;
    for(int j=0;j< m_dataAllVec[47].m_edgeParam.cols;j++)
    {
        paras1.push_back(m_dataAllVec[47].m_edgeParam.at<int>(0,j));
        paras2.push_back(m_dataAllVec[48].m_edgeParam.at<int>(1,j));
        paras8.push_back(m_dataAllVec[49].m_edgeParam.at<int>(2,j));
        paras9.push_back(m_dataAllVec[50].m_edgeParam.at<int>(3,j));
    }
    omp_set_num_threads(6);
#pragma omp parallel sections
    {
        #pragma omp section
        {
             TemplateMatch(srcImg,m_dataAllVec[47],rangSE1Vec);
             edgedetect obj;
             obj.NewKirschEdgeOuter(srcImg(rangSE1Vec[0]),paras1,pt1Vec);
             for(auto &pt:pt1Vec)
             {
                 pt.x+=rangSE1Vec[0].x;
                 pt.y+=rangSE1Vec[0].y;
             }
        }
        #pragma omp section
        {
             TemplateMatch(srcImg,m_dataAllVec[48],rangSE2Vec);
             edgedetect obj;
             obj.NewKirschEdgeOuter(srcImg(rangSE2Vec[0]),paras2,pt2Vec);
             for(auto &pt:pt2Vec)
             {
                 pt.x+=rangSE2Vec[0].x;
                 pt.y+=rangSE2Vec[0].y;
             }
        }
        #pragma omp section
        {
             TemplateMatch(srcImg,m_dataAllVec[49],rangSE8Vec);
             edgedetect obj;
             obj.NewKirschEdgeOuter(srcImg(rangSE8Vec[0]),paras8,pt8Vec);
             for(auto &pt:pt8Vec)
             {
                 pt.x+=rangSE8Vec[0].x;
                 pt.y+=rangSE8Vec[0].y;
             }
        }
        #pragma omp section
        {
             TemplateMatch(srcImg,m_dataAllVec[50],rangSE9Vec);
             edgedetect obj;
             obj.NewKirschEdgeOuter(srcImg(rangSE9Vec[0]),paras9,pt9Vec);
             for(auto &pt:pt9Vec)
             {
                 pt.x+=rangSE9Vec[0].x;
                 pt.y+=rangSE9Vec[0].y;
             }
        }

    }

    std::unique_lock<std::mutex> lock(*AlgBase::mtxptrVec[id]);
    while (!AlgBase::flagVec[id])
    {
        (*AlgBase::cvarptrVec[id]).wait(lock);
    }
#pragma omp parallel sections
    {
        #pragma omp section
        {
            for(auto &pt2:pt1Vec)
            {
                cv::Point2f ptsOut;
                CoordinateTransformationRelateToDot(datumBackVec[id], pt2,ptsOut);
                pt2=ptsOut;
            }
         }

        #pragma omp section
        {
            for(auto &pt2:pt2Vec)
            {
                cv::Point2f ptsOut;
                CoordinateTransformationRelateToDot(datumBackVec[id], pt2,ptsOut);
                pt2=ptsOut;
            }
         }
        #pragma omp section
        {
            for(auto &pt2:pt8Vec)
            {
                cv::Point2f ptsOut;
                CoordinateTransformationRelateToDot(datumBackVec[id], pt2,ptsOut);
                pt2=ptsOut;
            }
         }

        #pragma omp section
        {
            for(auto &pt2:pt9Vec)
            {
                cv::Point2f ptsOut;
                CoordinateTransformationRelateToDot(datumBackVec[id], pt2,ptsOut);
                pt2=ptsOut;
            }
         }
     }

    float fai6se1=0;
    for(int i=0;i<pt1Vec.size();++i) {
        fai6se1+=pt1Vec[i].y/pt1Vec.size();
    }

    std::string prefix = m_xmlFileNamePre + to_string(fixtureId) + "_";
    float fai6se9 = 0;
    for(int i = 0; i < pt9Vec.size(); ++i) {
        std::string fai6Se1Se9Str = string("FAI6-SE1-SE9-Y" + std::to_string(i+1) + "-distance");
        measRes.insert(std::make_pair(string(fai6Se1Se9Str + "_pixel"), fabs(pt9Vec[i].y - pt1Vec[i].y)));
        float fai6Se1Se9y = CalcMeasureDistance(m_phypixpara[prefix + fai6Se1Se9Str], fabs(pt9Vec[i].y - pt1Vec[i].y));
        measRes.insert(std::make_pair(string(fai6Se1Se9Str + "_real"), fai6Se1Se9y));
        fai6se9 += pt9Vec[i].y/pt9Vec.size();
    }

    measRes.insert(std::make_pair(string("FAI6-SE1-SE9-Y-distance_pixel"), fabs(fai6se1-fai6se9)));
    float fai6 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI6-SE1-SE9-Y-distance")], fabs(fai6se1-fai6se9));
    measRes.insert(std::make_pair(string("FAI6-SE1-SE9-Y-distance_real"), fai6));

    float fai232se2=0;
    for(int i=0;i<pt2Vec.size();++i){
        fai232se2+=pt2Vec[i].y/pt2Vec.size();
    }
    measRes.insert(std::make_pair(string("FAI232-SE1-SE2-Y-distance_pixel"), fabs(fai6se1-fai232se2)));
    float fai232 = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI232-SE1-SE2-Y-distance")], fabs(fai6se1-fai232se2));
    measRes.insert(std::make_pair(string("FAI232-SE1-SE2-Y-distance_real"), fai232));
}
